#include "uwbautofollow/psd.h"

// park sensor position
//          4   3  2   1
//           \  |  |  /
//       5 --          -- 0
//       6 --          -- 11
//          /   |  |  \
//         7    8  9   10
// left postive, righ negtive

// park sensor position
//      left          right
//           2   1   0   (  4  3  2  )
//            \  |  /
//  (5)  3 --          -- 11  (1)
//       4 --          -- 10
//       5 --          -- 9
//           /   |   \
//          6    7    8
//         (6)
// left postive, righ negtive

ParkSensorDistance::ParkSensorDistance()
{
    raw_idx1 = 0;
    raw_idx2 = 0;
    for (int ii = 0; ii < SENSOR_NUMS; ii++)
    {
        for (int jj = 0; jj < FLT_SIZE; jj++)
        {
            raw_distances[ii][jj] = 0xFF;
        }

        filter_distances[ii] = 0xFF;

        sensor_position[ii] = ii;
    }
}

ParkSensorDistance::~ParkSensorDistance()
{
}

void ParkSensorDistance::setSensorPosition(int position[SENSOR_NUMS])
{
    for (int ii = 0; ii < SENSOR_NUMS; ii++)
    {
        sensor_position[ii] = position[ii];
    }
}

void ParkSensorDistance::setDistanceFront(int psd1, int psd2, int psd3, int psd4, int psd5, int psd6)
{
    raw_distances[0][raw_idx1] = psd1;
    raw_distances[1][raw_idx1] = psd2;
    raw_distances[2][raw_idx1] = psd3;
    raw_distances[3][raw_idx1] = psd4;
    raw_distances[4][raw_idx1] = psd5;
    raw_distances[5][raw_idx1] = psd6;

    raw_idx1++;
    if (raw_idx1 >= FLT_SIZE)
    {
        raw_idx1 = 0;
    }

    for (int i = 0; i < 6; i++)
    {
        filter_distances[i] = min_distance(i);
    }
}

void ParkSensorDistance::setDistanceBack(int psd7, int psd8, int psd9, int psd10, int psd11, int psd12)
{
    raw_distances[6][raw_idx2] = psd7;
    raw_distances[7][raw_idx2] = psd8;
    raw_distances[8][raw_idx2] = psd9;
    raw_distances[9][raw_idx2] = psd10;
    raw_distances[10][raw_idx2] = psd11;
    raw_distances[11][raw_idx2] = psd12;

    raw_idx2++;
    if (raw_idx2 >= FLT_SIZE)
    {
        raw_idx2 = 0;
    }

    for (int i = 6; i < 12; i++)
    {
        filter_distances[i] = min_distance(i);
    }
}

bool ParkSensorDistance::lessThan(int position, float distance)
{

    int dist = getDistance(position);

    if (dist == 0xFF) // num 6 ultrasonic always returns 0xFF
        return false;

    if (dist == 0xFE)
        return false;

    if ((dist < 0xFB) && (dist * 2 / 100.0f < distance))
        return true;

    return false;
}

//"Park Sensor Distance
// self-checking ERROR:FF
// self-checking Normal:FE
// NO obstacle:FE
// 0xFB~0xFD:NO USED "

bool ParkSensorDistance::isValid(int position)
{
    if (position < 0 || position >= SENSOR_NUMS)
        return false;

    int id = sensor_position[position];
    if (id < 0 || id >= SENSOR_NUMS)
    {
        return false;
    }
    int dist = getDistance(position);
    if (dist >= 0 && (dist == 0xFF || dist == 0xFE || dist < 0xFB))
    {
        return true;
    }
    return false;
}

int ParkSensorDistance::getDistance(int position)
{
    if (position < 0 || position >= SENSOR_NUMS)
        return 0xFF;

    int id = sensor_position[position];
    if (id < 0 || id >= SENSOR_NUMS)
    {
        return 0xFF;
    }

    return filter_distances[id];
}

int ParkSensorDistance::min_distance(int id)
{
    int tmp = raw_distances[id][0];
    for (int i = 1; i < FLT_SIZE; i++)
    {
        if (tmp > raw_distances[id][i])
            tmp = raw_distances[id][i];
    }

    return tmp;
}

bool ParkSensorDistance::findLeftWayOut(int sensorId, int safe_distance)
{
  bool found = false;
  if (!psd_side_danger(sensorId)) // then check the left sensor
  {
    found = true;
  }
  return found;
}

bool ParkSensorDistance::findRightWayOut(int sensorId, int safe_distance)
{
  bool found = false;

  if (!psd_side_danger(sensorId)) // check the right sensor first
  {
    found = true;
  }

  return found;
}

bool ParkSensorDistance::findWayOut(int leftSensorId, int rightSensorId, int safe_distance)
{
  bool found = false;
    found = findRightWayOut(rightSensorId, safe_distance);
    if (!found)
      found = findLeftWayOut(leftSensorId, safe_distance);

  return found;
}

void ParkSensorDistance::Avoid_Danger(void)
{
  
}

void ParkSensorDistance::psd_filter()
{
  
}
void ParkSensorDistance::psd_tick()
{
  
}

bool ParkSensorDistance::psd_danger(int sensorId)
{
    return false;
}
bool ParkSensorDistance::psd_side_danger(int sensorId)
{
    return false;
}
